#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, Float64, String
import numpy as np
import argparse

def main():
    parser = argparse.ArgumentParser(description='Send pose commands repeatedly.')
    parser.add_argument('--repeat', type=int, default=1,
                        help='Number of times to toggle pose commands')
    args = parser.parse_args()

    rospy.init_node('move_nullspace_commander', anonymous=True)
    pub_joint = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)
    pub_pose = rospy.Publisher('/command_move_nullspace', Float64, queue_size=10)

    rospy.sleep(1)

    msg = Float64MultiArray()
    start_joint = [-2.086099624633789, 0.4533843994140625, 0.6681547164916992, -1.546311378479004, 0.18787670135498047, -0.3416109085083008, -0.3828105926513672]

    print("move to start point...")
    msg.data = start_joint
    pub_joint.publish(msg)
    rospy.sleep(5.5)

    msg_nullspace = Float64()

    for i in range(args.repeat):
        print(f"[{i+1}/{args.repeat}] move nullspace clock-wise...")
        msg_nullspace.data = 0.2
        pub_pose.publish(msg_nullspace)
        rospy.sleep(2)

        print(f"[{i+1}/{args.repeat}] move nullspace counterclock-wise...")
        msg_nullspace.data = -0.4
        pub_pose.publish(msg_nullspace)
        rospy.sleep(2)

        print(f"[{i+1}/{args.repeat}] move nullspace clock-wise...")
        msg_nullspace.data = 0.2
        pub_pose.publish(msg_nullspace)
        rospy.sleep(2)

if __name__ == '__main__':
    main()


